package com.yuneec.mavlink;

import com.yuneec.droneservice.DroneService;
import com.yuneec.droneservice.ServiceManager;
import com.yuneec.droneservice.protocol.ProtocolPlugin;
import com.yuneec.droneservice.protocol.analysis.Analysis;
import com.yuneec.droneservice.protocol.analysis.ByteBuf;
import com.yuneec.droneservice.protocol.analysis.IMessageReceiver;
import com.yuneec.droneservice.request.IRequest;
import com.yuneec.droneservice.request.IResponseHandler;
import com.yuneec.droneservice.request.OnListener;
import com.yuneec.droneservice.tools.RCLog;
import com.yuneec.rcdronesercive.RCProtocolPlugin;

import java.util.LinkedList;

public class MavLinkProtocolPlugin implements ProtocolPlugin {
    public static final int L_MAVLINK = 0x100;
    public static final int MAVLINK_MSG_ID = 10011;
    private final RCProtocolPlugin lcmPlugin = new RCProtocolPlugin();
    private LinkedList<OnListener<ByteBuf>> mavlinkOnListener;

    @Override
    public IResponseHandler handleResponseBuf(byte[] data, int offset, int length) {
        return lcmPlugin.handleResponseBuf(data, offset, length);
    }

    @Override
    public byte[] toRawData(IRequest request) {
        return lcmPlugin.toRawData(request);
    }

    @Override
    public int getPort() {
        return lcmPlugin.getPort();
    }

    @Override
    public void init(DroneService.DroneBinder mBinder) {
        lcmPlugin.init(mBinder);
        mBinder.unregistStreamReceive(MAVLINK_MSG_ID);
        mavlinkOnListener = new LinkedList<>();
        mBinder.registStreamReceive(new IMessageReceiver() {
            private ByteBuf buf;

            @Override
            public void handleMessage(Analysis.Message msg) {
                if (msg == null) {
                    return;
                }
                try {
                    if (buf == null) {
                        buf = new ByteBuf(msg.body(), msg.offset(), msg.length());
                    } else {
                        buf.buf = msg.body();
                        buf.offset = msg.offset();
                        buf.length = msg.length();
                    }
                    for (OnListener<ByteBuf> onListener : mavlinkOnListener) {
                        if (onListener != null) {
                            onListener.onListener(buf);
                        }
                    }
                } catch (Exception e) {
                    catchException(e);
                }
            }

            @Override
            public int getMessageId() {
                return MAVLINK_MSG_ID;
            }

            @Override
            public void catchException(Throwable e) {
                RCLog.d("MavLinkProtocolPlugin", e);
            }
        });
    }

    private final static LinkedList<OnListener<ByteBuf>> getListeners() throws Exception {
        ProtocolPlugin pp = ServiceManager.getInstance().getProtocolPlugin();
        if (pp == null || !(pp instanceof MavLinkProtocolPlugin)) {
            throw new Exception("Misuse protocol plugin!" + pp);
        }
        MavLinkProtocolPlugin mpp = MavLinkProtocolPlugin.class.cast(ServiceManager.getInstance().getProtocolPlugin());
        LinkedList<OnListener<ByteBuf>> listeners = mpp.mavlinkOnListener;
        if (listeners == null) {
            throw new Exception("ProtocolPlug do not init!");
        }
        return listeners;
    }

    public static void clearMavLinkReceive() {
        try {
            getListeners().clear();
        } catch (Exception e) {
            RCLog.d("MavLinkProtocolPlugin.clearMavLinkReceive", e);
        }
    }

    public static void addMavLinkReceive(OnListener<ByteBuf> listener) {
        if (listener == null) {
            return;
        }
        try {
            getListeners().addLast(listener);
            ServiceManager.getInstance().writeAOAData(MAVLINK_MSG_ID, "Hello".getBytes());
        } catch (Exception e) {
            RCLog.d("MavLinkProtocolPlugin.addMavLinkReceive", e);
        }
    }

    public static void removeMavLinkReceive(OnListener<ByteBuf> listener) {
        if (listener == null) {
            return;
        }
        try {
            getListeners().remove(listener);
        } catch (Exception e) {
            RCLog.d("MavLinkProtocolPlugin.removeMavLinkReceive", e);
        }
    }
}
